#ifndef __MAT3X4_H__
#define __MAT3X4_H__

/*
===============================================================================

row-major 3x4 matrix

Mat3 m;
Vec3 t;

m[0][0], m[1][0], m[2][0], t[0]
m[0][1], m[1][1], m[2][1], t[1]
m[0][2], m[1][2], m[2][2], t[2]

===============================================================================
*/

namespace inf {

    class Mat3x4 {
    public:

        void			set_rotation( const Mat3 &m );
        void			set_translation( const Vec3 &t );

        bool			compare( const Mat3x4 &a ) const;							// exact compare, no epsilon
        bool			compare( const Mat3x4 &a, const float epsilon ) const;	// compare with epsilon
        bool			operator==(	const Mat3x4 &a ) const;						// exact compare, no epsilon
        bool			operator!=(	const Mat3x4 &a ) const;						// exact compare, no epsilon

        void			identity( void );
        void			invert( void );

        void			left_multiply( const Mat3x4 &m );
        void			left_multiply( const Mat3 &m );
        void			right_multiply( const Mat3x4 &m );
        void			right_multiply( const Mat3 &m );

        void			transform( Vec3 &result, const Vec3 &v ) const;
        void			rotate( Vec3 &result, const Vec3 &v ) const;

        Mat3			to_Mat3( void ) const;
        Vec3			to_Vec3( void ) const;
        const float *	to_float_ptr( void ) const;
        float *			to_float_ptr( void );
        const char *	to_string( int precision = 2 ) const;

    private:
        float			mat[3*4];
    };


    INF_INLINE void Mat3x4::set_rotation( const Mat3 &m ) {
        // NOTE: Mat3 is transposed because it is column-major
        mat[0 * 4 + 0] = m[0][0];
        mat[0 * 4 + 1] = m[1][0];
        mat[0 * 4 + 2] = m[2][0];
        mat[1 * 4 + 0] = m[0][1];
        mat[1 * 4 + 1] = m[1][1];
        mat[1 * 4 + 2] = m[2][1];
        mat[2 * 4 + 0] = m[0][2];
        mat[2 * 4 + 1] = m[1][2];
        mat[2 * 4 + 2] = m[2][2];
    }

    INF_INLINE void Mat3x4::set_translation( const Vec3 &t ) {
        mat[0 * 4 + 3] = t[0];
        mat[1 * 4 + 3] = t[1];
        mat[2 * 4 + 3] = t[2];
    }

    INF_INLINE bool Mat3x4::compare( const Mat3x4 &a ) const {
        int i;

        for ( i = 0; i < 12; i++ ) {
            if ( mat[i] != a.mat[i] ) {
                return false;
            }
        }
        return true;
    }

    INF_INLINE bool Mat3x4::compare( const Mat3x4 &a, const float epsilon ) const {
        int i;

        for ( i = 0; i < 12; i++ ) {
            if ( math::fabs( mat[i] - a.mat[i] ) > epsilon ) {
                return false;
            }
        }
        return true;
    }

    INF_INLINE bool Mat3x4::operator==( const Mat3x4 &a ) const {
        return compare( a );
    }

    INF_INLINE bool Mat3x4::operator!=( const Mat3x4 &a ) const {
        return !compare( a );
    }

    INF_INLINE void Mat3x4::identity( void ) {
        mat[0 * 4 + 0] = 1.0f; mat[0 * 4 + 1] = 0.0f; mat[0 * 4 + 2] = 0.0f; mat[0 * 4 + 3] = 0.0f;
        mat[0 * 4 + 0] = 0.0f; mat[0 * 4 + 1] = 1.0f; mat[0 * 4 + 2] = 0.0f; mat[0 * 4 + 3] = 0.0f;
        mat[0 * 4 + 0] = 0.0f; mat[0 * 4 + 1] = 0.0f; mat[0 * 4 + 2] = 1.0f; mat[0 * 4 + 3] = 0.0f;
    }

    INF_INLINE void Mat3x4::invert( void ) {
        float tmp[3];

        // negate inverse rotated translation part
        tmp[0] = mat[0 * 4 + 0] * mat[0 * 4 + 3] + mat[1 * 4 + 0] * mat[1 * 4 + 3] + mat[2 * 4 + 0] * mat[2 * 4 + 3];
        tmp[1] = mat[0 * 4 + 1] * mat[0 * 4 + 3] + mat[1 * 4 + 1] * mat[1 * 4 + 3] + mat[2 * 4 + 1] * mat[2 * 4 + 3];
        tmp[2] = mat[0 * 4 + 2] * mat[0 * 4 + 3] + mat[1 * 4 + 2] * mat[1 * 4 + 3] + mat[2 * 4 + 2] * mat[2 * 4 + 3];
        mat[0 * 4 + 3] = -tmp[0];
        mat[1 * 4 + 3] = -tmp[1];
        mat[2 * 4 + 3] = -tmp[2];

        // transpose rotation part
        tmp[0] = mat[0 * 4 + 1];
        mat[0 * 4 + 1] = mat[1 * 4 + 0];
        mat[1 * 4 + 0] = tmp[0];
        tmp[1] = mat[0 * 4 + 2];
        mat[0 * 4 + 2] = mat[2 * 4 + 0];
        mat[2 * 4 + 0] = tmp[1];
        tmp[2] = mat[1 * 4 + 2];
        mat[1 * 4 + 2] = mat[2 * 4 + 1];
        mat[2 * 4 + 1] = tmp[2];
    }

    INF_INLINE void Mat3x4::left_multiply( const Mat3x4 &m ) {
        float t0, t1;

        t0				= m.mat[0 * 4 + 0] * mat[0 * 4 + 0] + m.mat[0 * 4 + 1] * mat[1 * 4 + 0] + m.mat[0 * 4 + 2] * mat[2 * 4 + 0];
        t1				= m.mat[1 * 4 + 0] * mat[0 * 4 + 0] + m.mat[1 * 4 + 1] * mat[1 * 4 + 0] + m.mat[1 * 4 + 2] * mat[2 * 4 + 0];
        mat[2 * 4 + 0]	= m.mat[2 * 4 + 0] * mat[0 * 4 + 0] + m.mat[2 * 4 + 1] * mat[1 * 4 + 0] + m.mat[2 * 4 + 2] * mat[2 * 4 + 0];

        mat[1 * 4 + 0] = t1;
        mat[0 * 4 + 0] = t0;

        t0				= m.mat[0 * 4 + 0] * mat[0 * 4 + 1] + m.mat[0 * 4 + 1] * mat[1 * 4 + 1] + m.mat[0 * 4 + 2] * mat[2 * 4 + 1];
        t1				= m.mat[1 * 4 + 0] * mat[0 * 4 + 1] + m.mat[1 * 4 + 1] * mat[1 * 4 + 1] + m.mat[1 * 4 + 2] * mat[2 * 4 + 1];
        mat[2 * 4 + 1]	= m.mat[2 * 4 + 0] * mat[0 * 4 + 1] + m.mat[2 * 4 + 1] * mat[1 * 4 + 1] + m.mat[2 * 4 + 2] * mat[2 * 4 + 1];

        mat[1 * 4 + 1] = t1;
        mat[0 * 4 + 1] = t0;

        t0				= m.mat[0 * 4 + 0] * mat[0 * 4 + 2] + m.mat[0 * 4 + 1] * mat[1 * 4 + 2] + m.mat[0 * 4 + 2] * mat[2 * 4 + 2];
        t1				= m.mat[1 * 4 + 0] * mat[0 * 4 + 2] + m.mat[1 * 4 + 1] * mat[1 * 4 + 2] + m.mat[1 * 4 + 2] * mat[2 * 4 + 2];
        mat[2 * 4 + 2]	= m.mat[2 * 4 + 0] * mat[0 * 4 + 2] + m.mat[2 * 4 + 1] * mat[1 * 4 + 2] + m.mat[2 * 4 + 2] * mat[2 * 4 + 2];

        mat[1 * 4 + 2] = t1;
        mat[0 * 4 + 2] = t0;

        t0				= m.mat[0 * 4 + 0] * mat[0 * 4 + 3] + m.mat[0 * 4 + 1] * mat[1 * 4 + 3] + m.mat[0 * 4 + 2] * mat[2 * 4 + 3] + m.mat[0 * 4 + 3];
        t1				= m.mat[1 * 4 + 0] * mat[0 * 4 + 3] + m.mat[1 * 4 + 1] * mat[1 * 4 + 3] + m.mat[1 * 4 + 2] * mat[2 * 4 + 3] + m.mat[1 * 4 + 3];
        mat[2 * 4 + 3]	= m.mat[2 * 4 + 0] * mat[0 * 4 + 3] + m.mat[2 * 4 + 1] * mat[1 * 4 + 3] + m.mat[2 * 4 + 2] * mat[2 * 4 + 3] + m.mat[2 * 4 + 3];

        mat[1 * 4 + 3] = t1;
        mat[0 * 4 + 3] = t0;
    }

    INF_INLINE void Mat3x4::left_multiply( const Mat3 &m ) {
        float t0, t1;

        // NOTE: Mat3 is column-major
        t0				= m[0][0] * mat[0 * 4 + 0] + m[1][0] * mat[1 * 4 + 0] + m[2][0] * mat[2 * 4 + 0];
        t1				= m[0][1] * mat[0 * 4 + 0] + m[1][1] * mat[1 * 4 + 0] + m[2][1] * mat[2 * 4 + 0];
        mat[2 * 4 + 0]	= m[0][2] * mat[0 * 4 + 0] + m[1][2] * mat[1 * 4 + 0] + m[2][2] * mat[2 * 4 + 0];

        mat[1 * 4 + 0] = t1;
        mat[0 * 4 + 0] = t0;

        t0				= m[0][0] * mat[0 * 4 + 1] + m[1][0] * mat[1 * 4 + 1] + m[2][0] * mat[2 * 4 + 1];
        t1				= m[0][1] * mat[0 * 4 + 1] + m[1][1] * mat[1 * 4 + 1] + m[2][1] * mat[2 * 4 + 1];
        mat[2 * 4 + 1]	= m[0][2] * mat[0 * 4 + 1] + m[1][2] * mat[1 * 4 + 1] + m[2][2] * mat[2 * 4 + 1];

        mat[1 * 4 + 1] = t1;
        mat[0 * 4 + 1] = t0;

        t0				= m[0][0] * mat[0 * 4 + 2] + m[1][0] * mat[1 * 4 + 2] + m[2][0] * mat[2 * 4 + 2];
        t1				= m[0][1] * mat[0 * 4 + 2] + m[1][1] * mat[1 * 4 + 2] + m[2][1] * mat[2 * 4 + 2];
        mat[2 * 4 + 2]	= m[0][2] * mat[0 * 4 + 2] + m[1][2] * mat[1 * 4 + 2] + m[2][2] * mat[2 * 4 + 2];

        mat[1 * 4 + 2] = t1;
        mat[0 * 4 + 2] = t0;

        t0				= m[0][0] * mat[0 * 4 + 3] + m[1][0] * mat[1 * 4 + 3] + m[2][0] * mat[2 * 4 + 3];
        t1				= m[0][1] * mat[0 * 4 + 3] + m[1][1] * mat[1 * 4 + 3] + m[2][1] * mat[2 * 4 + 3];
        mat[2 * 4 + 3]	= m[0][2] * mat[0 * 4 + 3] + m[1][2] * mat[1 * 4 + 3] + m[2][2] * mat[2 * 4 + 3];

        mat[1 * 4 + 3] = t1;
        mat[0 * 4 + 3] = t0;
    }

    INF_INLINE void Mat3x4::right_multiply( const Mat3x4 &m ) {
        float t0, t1, t2;

        t0				= mat[0 * 4 + 0] * m.mat[0 * 4 + 0] + mat[0 * 4 + 1] * m.mat[1 * 4 + 0] + mat[0 * 4 + 2] * m.mat[2 * 4 + 0];
        t1				= mat[0 * 4 + 0] * m.mat[0 * 4 + 1] + mat[0 * 4 + 1] * m.mat[1 * 4 + 1] + mat[0 * 4 + 2] * m.mat[2 * 4 + 1];
        t2				= mat[0 * 4 + 0] * m.mat[0 * 4 + 2] + mat[0 * 4 + 1] * m.mat[1 * 4 + 2] + mat[0 * 4 + 2] * m.mat[2 * 4 + 2];
        mat[0 * 4 + 3]	= mat[0 * 4 + 0] * m.mat[0 * 4 + 3] + mat[0 * 4 + 1] * m.mat[1 * 4 + 3] + mat[0 * 4 + 2] * m.mat[2 * 4 + 3] + mat[0 * 4 + 3];

        mat[0 * 4 + 0] = t0;
        mat[0 * 4 + 1] = t1;
        mat[0 * 4 + 2] = t2;

        t0				= mat[1 * 4 + 0] * m.mat[0 * 4 + 0] + mat[1 * 4 + 1] * m.mat[1 * 4 + 0] + mat[1 * 4 + 2] * m.mat[2 * 4 + 0];
        t1				= mat[1 * 4 + 0] * m.mat[0 * 4 + 1] + mat[1 * 4 + 1] * m.mat[1 * 4 + 1] + mat[1 * 4 + 2] * m.mat[2 * 4 + 1];
        t2				= mat[1 * 4 + 0] * m.mat[0 * 4 + 2] + mat[1 * 4 + 1] * m.mat[1 * 4 + 2] + mat[1 * 4 + 2] * m.mat[2 * 4 + 2];
        mat[1 * 4 + 3]	= mat[1 * 4 + 0] * m.mat[0 * 4 + 3] + mat[1 * 4 + 1] * m.mat[1 * 4 + 3] + mat[1 * 4 + 2] * m.mat[2 * 4 + 3] + mat[1 * 4 + 3];

        mat[1 * 4 + 0] = t0;
        mat[1 * 4 + 1] = t1;
        mat[1 * 4 + 2] = t2;

        t0				= mat[2 * 4 + 0] * m.mat[0 * 4 + 0] + mat[2 * 4 + 1] * m.mat[1 * 4 + 0] + mat[2 * 4 + 2] * m.mat[2 * 4 + 0];
        t1				= mat[2 * 4 + 0] * m.mat[0 * 4 + 1] + mat[2 * 4 + 1] * m.mat[1 * 4 + 1] + mat[2 * 4 + 2] * m.mat[2 * 4 + 1];
        t2				= mat[2 * 4 + 0] * m.mat[0 * 4 + 2] + mat[2 * 4 + 1] * m.mat[1 * 4 + 2] + mat[2 * 4 + 2] * m.mat[2 * 4 + 2];
        mat[2 * 4 + 3]	= mat[2 * 4 + 0] * m.mat[0 * 4 + 3] + mat[2 * 4 + 1] * m.mat[1 * 4 + 3] + mat[2 * 4 + 2] * m.mat[2 * 4 + 3] + mat[2 * 4 + 3];

        mat[2 * 4 + 0] = t0;
        mat[2 * 4 + 1] = t1;
        mat[2 * 4 + 2] = t2;
    }

    INF_INLINE void Mat3x4::right_multiply( const Mat3 &m ) {
        float t0, t1, t2;

        // NOTE: Mat3 is column-major
        t0				= mat[0 * 4 + 0] * m[0][0] + mat[0 * 4 + 1] * m[0][1] + mat[0 * 4 + 2] * m[0][2];
        t1				= mat[0 * 4 + 0] * m[1][0] + mat[0 * 4 + 1] * m[1][1] + mat[0 * 4 + 2] * m[1][2];
        t2				= mat[0 * 4 + 0] * m[2][0] + mat[0 * 4 + 1] * m[2][1] + mat[0 * 4 + 2] * m[2][2];

        mat[0 * 4 + 0] = t0;
        mat[0 * 4 + 1] = t1;
        mat[0 * 4 + 2] = t2;

        t0				= mat[1 * 4 + 0] * m[0][0] + mat[1 * 4 + 1] * m[0][1] + mat[1 * 4 + 2] * m[0][2];
        t1				= mat[1 * 4 + 0] * m[1][0] + mat[1 * 4 + 1] * m[1][1] + mat[1 * 4 + 2] * m[1][2];
        t2				= mat[1 * 4 + 0] * m[2][0] + mat[1 * 4 + 1] * m[2][1] + mat[1 * 4 + 2] * m[2][2];

        mat[1 * 4 + 0] = t0;
        mat[1 * 4 + 1] = t1;
        mat[1 * 4 + 2] = t2;

        t0				= mat[2 * 4 + 0] * m[0][0] + mat[2 * 4 + 1] * m[0][1] + mat[2 * 4 + 2] * m[0][2];
        t1				= mat[2 * 4 + 0] * m[1][0] + mat[2 * 4 + 1] * m[1][1] + mat[2 * 4 + 2] * m[1][2];
        t2				= mat[2 * 4 + 0] * m[2][0] + mat[2 * 4 + 1] * m[2][1] + mat[2 * 4 + 2] * m[2][2];

        mat[2 * 4 + 0] = t0;
        mat[2 * 4 + 1] = t1;
        mat[2 * 4 + 2] = t2;
    }

    INF_INLINE void Mat3x4::transform( Vec3 &result, const Vec3 &v ) const {
        result.x = mat[0 * 4 + 0] * v.x + mat[0 * 4 + 1] * v.y + mat[0 * 4 + 2] * v.z + mat[0 * 4 + 3];
        result.y = mat[1 * 4 + 0] * v.x + mat[1 * 4 + 1] * v.y + mat[1 * 4 + 2] * v.z + mat[1 * 4 + 3];
        result.z = mat[2 * 4 + 0] * v.x + mat[2 * 4 + 1] * v.y + mat[2 * 4 + 2] * v.z + mat[2 * 4 + 3];
    }

    INF_INLINE void Mat3x4::rotate( Vec3 &result, const Vec3 &v ) const {
        result.x = mat[0 * 4 + 0] * v.x + mat[0 * 4 + 1] * v.y + mat[0 * 4 + 2] * v.z;
        result.y = mat[1 * 4 + 0] * v.x + mat[1 * 4 + 1] * v.y + mat[1 * 4 + 2] * v.z;
        result.z = mat[2 * 4 + 0] * v.x + mat[2 * 4 + 1] * v.y + mat[2 * 4 + 2] * v.z;
    }

    INF_INLINE Mat3 Mat3x4::to_Mat3( void ) const {
        return Mat3(	mat[0 * 4 + 0], mat[1 * 4 + 0], mat[2 * 4 + 0],
            mat[0 * 4 + 1], mat[1 * 4 + 1], mat[2 * 4 + 1],
            mat[0 * 4 + 2], mat[1 * 4 + 2], mat[2 * 4 + 2] );
    }

    INF_INLINE Vec3 Mat3x4::to_Vec3( void ) const {
        return Vec3( mat[0 * 4 + 3], mat[1 * 4 + 3], mat[2 * 4 + 3] );
    }

    INF_INLINE const float *Mat3x4::to_float_ptr( void ) const {
        return mat;
    }

    INF_INLINE float *Mat3x4::to_float_ptr( void ) {
        return mat;
    }


}   // ::inf

#endif /* !__MAT3X4_H__ */
